#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <memory.h>
#include <unistd.h>
#include "iot_watchdog.h"
#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "hi_io.h"
#include "hi_time.h"
#include "iot_pwm.h"
#include "hi_pwm.h"
#include "iot_gpio_ex.h"
#include "robot_motor.h"
#include "robot_encoder.h"

#define GPIO0 0
#define GPIO1 1
#define GPIO9 9
#define GPIO10 10

#define IOT_PWM_PORT_PWM0 0
#define IOT_PWM_PORT_PWM1 1
#define IOT_PWM_PORT_PWM3 3
#define IOT_PWM_PORT_PWM4 4

#define ONE_SECOND    (1000)
#define PWM_FREQUENCY 32000 /*一般设置为16kHz*/

#define SPEED 0.15
#define CAR_RADIUS 0.10

/*将4个gpio复用为4路pwm输出*/
void gpioMotorControllerInit(void){
    hi_io_set_func(GPIO0, 5); 
    IoTGpioSetDir(GPIO0, IOT_GPIO_DIR_OUT);
    IoTPwmInit(IOT_PWM_PORT_PWM3);

    hi_io_set_func(GPIO1, 5); 
    IoTGpioSetDir(GPIO1, IOT_GPIO_DIR_OUT);
    IoTPwmInit(IOT_PWM_PORT_PWM4);

    hi_io_set_func(GPIO9, 5); 
    IoTGpioSetDir(GPIO9, IOT_GPIO_DIR_OUT);
    IoTPwmInit(IOT_PWM_PORT_PWM0);

    hi_io_set_func(GPIO10, 5); 
    IoTGpioSetDir(GPIO10, IOT_GPIO_DIR_OUT);
    IoTPwmInit(IOT_PWM_PORT_PWM1);
}

/* 前进 */
void car_forward(float distance) {
    hi_u32 time = distance* 1000/SPEED ; // 单位：ms
    setLeftMotorSpeed(SPEED);
    setRightMotorSpeed(SPEED);
    TaskMsleep(time);
    setLeftMotorSpeed(0);
    setRightMotorSpeed(0);
}

/* 后退 */
void car_backward(float distance) {
    hi_u32 time = distance* 1000/SPEED ; // 单位：ms
    setLeftMotorSpeed(-SPEED);
    setRightMotorSpeed(-SPEED);
    TaskMsleep(time);
    setLeftMotorSpeed(0);
    setRightMotorSpeed(0);
}

/* 原地左转 */
void car_turnLeft(int angle) {
    hi_u32 time = (angle*CAR_RADIUS*3.14)/(SPEED*180) * 1000; // 单位：ms
    setLeftMotorSpeed(-SPEED);
    setRightMotorSpeed(SPEED);
    TaskMsleep(time);
    setLeftMotorSpeed(0);
    setRightMotorSpeed(0);
}

/* 原地右转 */
void car_turnRight(int angle) {
    hi_u32 time = (angle*CAR_RADIUS*3.14)/(SPEED*180) * 1000; // 单位：ms
    setLeftMotorSpeed(SPEED);
    setRightMotorSpeed(-SPEED);
    TaskMsleep(time);
    setLeftMotorSpeed(0);
    setRightMotorSpeed(0);
}

/* 制动 */
void car_stop(void) {

    setLeftMotorSpeed(0);
    setRightMotorSpeed(0);

    IoTPwmStop(IOT_PWM_PORT_PWM3);
    IoTPwmStop(IOT_PWM_PORT_PWM4);
    IoTPwmStop(IOT_PWM_PORT_PWM0);
    IoTPwmStop(IOT_PWM_PORT_PWM1);

    IoTPwmStart(IOT_PWM_PORT_PWM3, 100, PWM_FREQUENCY);   //GPIO0
    IoTPwmStart(IOT_PWM_PORT_PWM4, 100, PWM_FREQUENCY); //GPIO1
    IoTPwmStart(IOT_PWM_PORT_PWM0, 100, PWM_FREQUENCY);   //GPIO9 
    IoTPwmStart(IOT_PWM_PORT_PWM1, 100, PWM_FREQUENCY); //GPIO10
}

/* pwm设定 */
void setLeftMotorPWM(int duty) {
    IoTPwmStop(IOT_PWM_PORT_PWM0);
    IoTPwmStop(IOT_PWM_PORT_PWM1);
    if (duty > 0){
        IoTPwmStart(IOT_PWM_PORT_PWM0, 0, PWM_FREQUENCY);   //GPIO9 
        IoTPwmStart(IOT_PWM_PORT_PWM1, duty, PWM_FREQUENCY);   //GPIO10
    }
    else{
        IoTPwmStart(IOT_PWM_PORT_PWM0, -duty, PWM_FREQUENCY);   //GPIO9 
        IoTPwmStart(IOT_PWM_PORT_PWM1, 0, PWM_FREQUENCY);   //GPIO10
    }
}


void setRightMotorPWM(int duty) {
    IoTPwmStop(IOT_PWM_PORT_PWM3);
    IoTPwmStop(IOT_PWM_PORT_PWM4);
    if (duty > 0){
        IoTPwmStart(IOT_PWM_PORT_PWM3, 0, PWM_FREQUENCY);   //GPIO0
        IoTPwmStart(IOT_PWM_PORT_PWM4, duty, PWM_FREQUENCY); //GPIO1
    }
    else{
        IoTPwmStart(IOT_PWM_PORT_PWM3, -duty, PWM_FREQUENCY);   //GPIO0
        IoTPwmStart(IOT_PWM_PORT_PWM4, 0, PWM_FREQUENCY); //GPIO1
    }
}

/* 从buf读取速度 */
int getSpeed (char* ret) {
    int speed = 0;
    for (int i = strlen(ret), j=0;i > 0;i--){
        if (ret[i] == ':') break;
        if (ret[i] <= 57 && ret[i] >= 48) { // 如果是数字
            speed = speed + (ret[i]-48)*pow(10,j);
            j++;
        }
        if (ret[i] == '-') speed = 0 - speed;
    }
    return speed;
}
